

#include <bsp_usart.h>
#include <rtthread.h>
#include <board.h>
#include <sbus.h>
#include <telegram.h>
#include <sensor_m8n.h>

// static uint8_t debug_uart_tx_buffer[DEBUG_UART_TX_BUFFER_SIZE];
static uint8_t debug_uart_rx_buffer[DEBUG_UART_RX_BUFFER_SIZE];
static volatile uint8_t debug_uart_rx_size = 0;
static volatile uint8_t debug_uart_rd_pos = 0;
static UART_HandleTypeDef debug_uart;
static rt_sem_t uart_sem = RT_NULL;

static UART_HandleTypeDef sbus_uart;
// static DMA_HandleTypeDef sbus_rx_dma;
static uint8_t sbus_rx_buffer[SBUS_UART_RX_BUFFER_SIZE];
static uint16_t sbus_rx_pos = 0;

static UART_HandleTypeDef telegram_uart;
static DMA_HandleTypeDef telegram_tx_dma;
// static DMA_HandleTypeDef telegram_rx_dma;
static uint8_t telegram_rx_buffer[TELEGRAM_UART_RX_BUFFER_SIZE];
static uint16_t telegram_rx_pos;

static UART_HandleTypeDef gps_uart;
static uint8_t gps_rx_buffer[GPS_UART_RX_BUFFER_SIZE];
static uint16_t gps_rx_pos;

static void debug_uart_msp_init(void)
{
    GPIO_InitTypeDef GPIO_InitStruct;

    /*##-1- Enable peripherals Clocks #################################*/
    /* Enable GPIO TX/RX clock */
    DEBUG_UART_GPIO_RCC_EN;

    /* Enable USARTx clock */
    DEBUG_UART_RCC_EN;

    /*##-2- Configure peripheral GPIO ##########################################*/
    /* UART TX GPIO pin configuration  */
    GPIO_InitStruct.Pin = DEBUG_UART_TX_PIN;
    GPIO_InitStruct.Mode = GPIO_MODE_AF_PP;
    GPIO_InitStruct.Pull = GPIO_PULLUP;
    GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_VERY_HIGH;
    GPIO_InitStruct.Alternate = DEBUG_UART_TX_RX_PIN_AF;

    HAL_GPIO_Init(DEBUG_UART_PIN_GROUP, &GPIO_InitStruct);

    /* UART RX GPIO pin configuration  */
    GPIO_InitStruct.Pin = DEBUG_UART_RX_PIN;
    GPIO_InitStruct.Alternate = DEBUG_UART_TX_RX_PIN_AF;

    HAL_GPIO_Init(DEBUG_UART_PIN_GROUP, &GPIO_InitStruct);

    /*##-4- Configure the NVIC for UART #########################################*/
}

/// @brief debug uart initialization
/// @param
/// @return
int bsp_debug_uart_init(void)
{
    debug_uart_msp_init();

    debug_uart.Instance = DEBUG_UART;
    debug_uart.Init.HwFlowCtl = UART_HWCONTROL_NONE;
    debug_uart.Init.BaudRate = 115200;
    debug_uart.Init.Mode = UART_MODE_TX_RX;
    debug_uart.Init.OverSampling = UART_OVERSAMPLING_16;
    debug_uart.Init.Parity = UART_PARITY_NONE;
    debug_uart.Init.StopBits = UART_STOPBITS_1;
    debug_uart.Init.WordLength = UART_WORDLENGTH_8B;
    HAL_UART_Init(&debug_uart);

    /* NVIC for USART, to catch the TX complete */
    HAL_NVIC_SetPriority(DEBUG_UART_IRQn, 1, 1);
    HAL_NVIC_EnableIRQ(DEBUG_UART_IRQn);

    __HAL_UART_ENABLE_IT(&debug_uart, UART_IT_RXNE); /* idle interrupt */

    return 0;
}

/// @brief 使用sbus的板级外设初始化
/// @param
static void sbus_uart_msp_init(void)
{
    GPIO_InitTypeDef GPIO_InitStruct;

    /*##-1- Enable peripherals Clocks #################################*/
    /* Enable GPIO TX/RX clock */
    SBUS_UART_GPIO_RCC_EN;

    /* Enable USARTx clock */
    SBUS_UART_RCC_EN;

    SBUS_DMA_RCC_EN;

    /*##-2- Configure peripheral GPIO ##*/
    /* UART RX GPIO pin configuration  */
    GPIO_InitStruct.Pin = SBUS_UART_RX_PIN;
    GPIO_InitStruct.Mode = GPIO_MODE_AF_PP;
    GPIO_InitStruct.Pull = GPIO_PULLUP;
    GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_HIGH;
    GPIO_InitStruct.Alternate = SBUS_UART_RX_PIN_AF;

    HAL_GPIO_Init(SBUS_UART_PIN_GROUP, &GPIO_InitStruct);
}

int bsp_sbus_uart_init(void)
{
    sbus_uart_msp_init();

    /* uart init */
    sbus_uart.Instance = SBUS_UART;
    sbus_uart.Init.HwFlowCtl = UART_HWCONTROL_NONE;
    sbus_uart.Init.BaudRate = SBUS_BAUDURATE;
    sbus_uart.Init.Mode = UART_MODE_RX;
    sbus_uart.Init.OverSampling = UART_OVERSAMPLING_16;
    sbus_uart.Init.Parity = SBUS_PARITY;
    sbus_uart.Init.StopBits = SBUS_STOPBIT;
    sbus_uart.Init.WordLength = SBUS_WORDLENGTH;
    HAL_UART_Init(&sbus_uart);
    HAL_NVIC_SetPriority(SBUS_UART_IRQn, 1, 1);
    HAL_NVIC_EnableIRQ(SBUS_UART_IRQn);
    __HAL_UART_CLEAR_FLAG(&sbus_uart, UART_FLAG_RXNE);

    /* DMA init */
    // sbus_rx_dma.Instance = SBUS_UART_RX_DMA_STREAM;
    // sbus_rx_dma.Init.Channel = SBUS_UART_RX_DMA_CHANNEL;
    // sbus_rx_dma.Init.Direction = DMA_PERIPH_TO_MEMORY;
    // sbus_rx_dma.Init.MemDataAlignment = DMA_MDATAALIGN_BYTE;
    // sbus_rx_dma.Init.FIFOMode = DMA_FIFOMODE_DISABLE;
    // sbus_rx_dma.Init.FIFOThreshold = DMA_FIFO_THRESHOLD_1QUARTERFULL;
    // sbus_rx_dma.Init.MemBurst = DMA_MBURST_SINGLE;
    // sbus_rx_dma.Init.MemInc = DMA_MINC_ENABLE;
    // sbus_rx_dma.Init.Mode = DMA_NORMAL;
    // sbus_rx_dma.Init.PeriphBurst = DMA_PBURST_SINGLE;
    // sbus_rx_dma.Init.PeriphDataAlignment = DMA_PDATAALIGN_BYTE;
    // sbus_rx_dma.Init.PeriphInc = DMA_PINC_DISABLE;
    // sbus_rx_dma.Init.Priority = DMA_PRIORITY_MEDIUM;
    // HAL_DMA_Init(&sbus_rx_dma);
    // __HAL_LINKDMA(&sbus_uart, hdmarx, sbus_rx_dma);
    // HAL_NVIC_SetPriority(SBUS_UART_DMA_RX_IRQn, 1, 1);
    // HAL_NVIC_EnableIRQ(SBUS_UART_DMA_RX_IRQn);

    return 0;
}

/// @brief 使用telegram的板级外设初始化
/// @param
static void telegram_uart_msp_init(void)
{
    GPIO_InitTypeDef GPIO_InitStruct;

    /*##-1- Enable peripherals Clocks #################################*/
    /* Enable GPIO TX/RX clock */
    TELEGRAM_UART_GPIO_RCC_EN;

    /* Enable USARTx clock */
    TELEGRAM_UART_RCC_EN;

    TELEGRAM_DMA_RCC_EN;

    /*##-2- Configure peripheral GPIO ##########################################*/
    /* UART RX GPIO pin configuration  */
    GPIO_InitStruct.Pin = TELEGRAM_UART_TX_PIN;
    GPIO_InitStruct.Mode = GPIO_MODE_AF_PP;
    GPIO_InitStruct.Pull = GPIO_PULLUP;
    GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_HIGH;
    GPIO_InitStruct.Alternate = TELEGRAM_UART_PIN_AF;

    HAL_GPIO_Init(TELEGRAM_UART_PIN_GROUP, &GPIO_InitStruct);

    /* SBUS connect to receiver pin configuration  */
    GPIO_InitStruct.Pin = TELEGRAM_UART_RX_PIN;
    GPIO_InitStruct.Alternate = TELEGRAM_UART_PIN_AF;

    HAL_GPIO_Init(TELEGRAM_UART_PIN_GROUP, &GPIO_InitStruct);
}

/// @brief 数传电台使用串口相关初始化
/// @param
/// @return
int bsp_telegram_uart_init(void)
{
    telegram_uart_msp_init();

    /* uart init */
    telegram_uart.Instance = TELEGRAM_UART;
    telegram_uart.Init.HwFlowCtl = UART_HWCONTROL_NONE;
    telegram_uart.Init.BaudRate = 115200;
    telegram_uart.Init.Mode = UART_MODE_TX_RX;
    telegram_uart.Init.OverSampling = UART_OVERSAMPLING_16;
    telegram_uart.Init.Parity = UART_PARITY_NONE;
    telegram_uart.Init.StopBits = UART_STOPBITS_1;
    telegram_uart.Init.WordLength = UART_WORDLENGTH_8B;
    HAL_UART_Init(&telegram_uart);
    HAL_NVIC_SetPriority(TELEGRAM_UART_IRQn, 1, 1);
    HAL_NVIC_EnableIRQ(TELEGRAM_UART_IRQn);
    __HAL_UART_CLEAR_FLAG(&telegram_uart, UART_FLAG_RXNE);

    /* DMA init */
    telegram_tx_dma.Instance = TELEGRAM_UART_TX_DMA_STREAM;
    telegram_tx_dma.Init.Channel = TELEGRAM_UART_TX_DMA_CHANNEL;
    telegram_tx_dma.Init.Direction = DMA_MEMORY_TO_PERIPH;
    telegram_tx_dma.Init.MemDataAlignment = DMA_MDATAALIGN_BYTE;
    telegram_tx_dma.Init.FIFOMode = DMA_FIFOMODE_DISABLE;
    telegram_tx_dma.Init.FIFOThreshold = DMA_FIFO_THRESHOLD_1QUARTERFULL;
    telegram_tx_dma.Init.MemBurst = DMA_MBURST_SINGLE;
    telegram_tx_dma.Init.MemInc = DMA_MINC_ENABLE;
    telegram_tx_dma.Init.Mode = DMA_NORMAL;
    telegram_tx_dma.Init.PeriphBurst = DMA_PBURST_SINGLE;
    telegram_tx_dma.Init.PeriphDataAlignment = DMA_PDATAALIGN_BYTE;
    telegram_tx_dma.Init.PeriphInc = DMA_PINC_DISABLE;
    telegram_tx_dma.Init.Priority = DMA_PRIORITY_MEDIUM;
    HAL_DMA_Init(&telegram_tx_dma);
    __HAL_LINKDMA(&telegram_uart, hdmatx, telegram_tx_dma);
    HAL_NVIC_SetPriority(TELEGRAM_UART_DMA_TX_IRQn, 1, 1);
    HAL_NVIC_EnableIRQ(TELEGRAM_UART_DMA_TX_IRQn);
    __HAL_DMA_ENABLE_IT(&telegram_tx_dma, DMA_IT_TC);

    // telegram_rx_dma.Instance = TELEGRAM_UART_RX_DMA_STREAM;
    // telegram_rx_dma.Init.Channel = TELEGRAM_UART_RX_DMA_CHANNEL;
    // telegram_rx_dma.Init.Direction = DMA_PERIPH_TO_MEMORY;
    // telegram_rx_dma.Init.MemDataAlignment = DMA_MDATAALIGN_BYTE;
    // telegram_rx_dma.Init.FIFOMode = DMA_FIFOMODE_DISABLE;
    // telegram_rx_dma.Init.FIFOThreshold = DMA_FIFO_THRESHOLD_1QUARTERFULL;
    // telegram_rx_dma.Init.MemBurst = DMA_MBURST_SINGLE;
    // telegram_rx_dma.Init.MemInc = DMA_MINC_ENABLE;
    // telegram_rx_dma.Init.Mode = DMA_NORMAL;
    // telegram_rx_dma.Init.PeriphBurst = DMA_PBURST_SINGLE;
    // telegram_rx_dma.Init.PeriphDataAlignment = DMA_PDATAALIGN_BYTE;
    // telegram_rx_dma.Init.PeriphInc = DMA_PINC_DISABLE;
    // telegram_rx_dma.Init.Priority = DMA_PRIORITY_MEDIUM;
    // HAL_DMA_Init(&telegram_rx_dma);
    // __HAL_LINKDMA(&telegram_uart, hdmarx, telegram_rx_dma);

    /* NVIC for USART, to catch the TX complete */

    // HAL_NVIC_SetPriority(TELEGRAM_UART_DMA_RX_IRQn, 1, 1);
    // HAL_NVIC_EnableIRQ(TELEGRAM_UART_DMA_RX_IRQn);

    return 0;
}

/// @brief 使用 gps 串口的板级MSP初始化
/// @param
static void gps_uart_msp_init(void)
{
    GPIO_InitTypeDef GPIO_InitStruct;

    /*##-1- Enable peripherals Clocks #################################*/
    /* Enable GPIO TX/RX clock */
    GPS_UART_GPIO_RCC_EN;

    /* Enable USARTx clock */
    GPS_UART_RCC_EN;

    /*##-2- Configure peripheral GPIO ##########################################*/
    /* UART RX GPIO pin configuration  */
    GPIO_InitStruct.Pin = GPS_UART_TX_PIN;
    GPIO_InitStruct.Mode = GPIO_MODE_AF_PP;
    GPIO_InitStruct.Pull = GPIO_PULLUP;
    GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_HIGH;
    GPIO_InitStruct.Alternate = GPS_UART_PIN_AF;

    HAL_GPIO_Init(GPS_UART_PIN_GROUP, &GPIO_InitStruct);

    /* SBUS connect to receiver pin configuration  */
    GPIO_InitStruct.Pin = GPS_UART_RX_PIN;
    GPIO_InitStruct.Alternate = GPS_UART_PIN_AF;

    HAL_GPIO_Init(GPS_UART_PIN_GROUP, &GPIO_InitStruct);
}

int bsp_gps_uart_init(void)
{
    gps_uart_msp_init();

    /* uart init */
    gps_uart.Instance = GPS_UART;
    gps_uart.Init.HwFlowCtl = UART_HWCONTROL_NONE;
    gps_uart.Init.BaudRate = 9600;
    gps_uart.Init.Mode = UART_MODE_TX_RX;
    gps_uart.Init.OverSampling = UART_OVERSAMPLING_16;
    gps_uart.Init.Parity = UART_PARITY_NONE;
    gps_uart.Init.StopBits = UART_STOPBITS_1;
    gps_uart.Init.WordLength = UART_WORDLENGTH_8B;
    HAL_UART_Init(&gps_uart);
    HAL_NVIC_SetPriority(GPS_UART_IRQn, 1, 1);
    HAL_NVIC_EnableIRQ(GPS_UART_IRQn);
    __HAL_UART_ENABLE_IT(&gps_uart, UART_IT_RXNE);
    __HAL_UART_CLEAR_FLAG(&gps_uart, UART_FLAG_RXNE);

    return 0;
}

/// @brief 返回GPS设备串口句柄
/// @return
UART_HandleTypeDef *bsp_gps_handle()
{
    return &gps_uart;
}

/// @brief 调用此函数发送数据到数传电台
/// @param tx_buf
/// @param tx_size
/// @return
int bsp_telegram_uart_tx(uint8_t *tx_buf, uint16_t tx_size)
{
    HAL_UART_Transmit(&telegram_uart, tx_buf, tx_size, tx_size);
    return 0;
}

/// @brief 调用此函数发送数据到控制台
/// @param tx_buf
/// @param tx_size
/// @return
int bsp_debug_uart_tx(uint8_t *tx_buf, uint16_t tx_size)
{
    HAL_UART_Transmit(&debug_uart, tx_buf, tx_size, tx_size * 5);
    return 0;
}

/// @brief re-implement console output string
/// @param str
void rt_hw_console_output(const char *str)
{
    uint16_t size = rt_strlen(str);
    uint16_t i = 0;
    char a = '\r';
    for (i = 0; i < size; i++)
    {
        if (*(str + i) == '\n')
        {
            HAL_UART_Transmit(&debug_uart, (uint8_t *)&a, 1, 1);
        }
        HAL_UART_Transmit(&debug_uart, (uint8_t *)(str + i), 1, 1);
    }
}

/// @brief re-implement console getchar
/// @param
/// @return
char rt_hw_console_getchar(void)
{
    char ch = -1;

    if (!uart_sem)
    {
        uart_sem = rt_sem_create("uart_sem", 0, RT_IPC_FLAG_PRIO);
    }
    if (uart_sem)
        rt_sem_take(uart_sem, RT_WAITING_FOREVER);

    if (debug_uart_rx_size == 0)
        return ch;

    if (debug_uart_rx_size == debug_uart_rd_pos)
    {
        debug_uart_rx_size = debug_uart_rd_pos = 0;
    }

    ch = debug_uart_rx_buffer[debug_uart_rd_pos];
    debug_uart_rd_pos++;

    return ch;
}

int sbus_run(void)
{
    __HAL_UART_ENABLE_IT(&sbus_uart, UART_IT_RXNE); /* idle interrupt */
    return 0;
}

int telegram_listen(void)
{
    __HAL_UART_ENABLE_IT(&telegram_uart, UART_IT_RXNE); /* idle interrupt */
    return 0;
}

/**
 * @brief This function handles debug uart global interrupt.
 */
/// @brief This function handles uart global interrupt.
/// @param
void DEBUG_UART_HANDLER(void)
{
    rt_interrupt_enter();

    HAL_UART_IRQHandler(&debug_uart);
    debug_uart_rx_buffer[debug_uart_rx_size] = debug_uart.Instance->DR & 0xff;
    debug_uart_rx_size++;

    if (uart_sem)
        rt_sem_release(uart_sem);

    rt_interrupt_leave();
}

/// @brief SBUS串口中断处理函数
/// @param
void SBUS_UART_HANDLER(void)
{
    rt_interrupt_enter();

    HAL_UART_IRQHandler(&sbus_uart);

    if (__HAL_UART_GET_FLAG(&sbus_uart, UART_FLAG_RXNE) == SET)
    {
        sbus_rx_buffer[sbus_rx_pos] = sbus_uart.Instance->DR & 0xff;
        sbus_rx_pos++;
        if (sbus_rx_pos == SBUS_FRAME_LEN * 2)
        {
            for (int i = 0; i < SBUS_FRAME_LEN; i++)
            {
                if (sbus_rx_buffer[i] == SBUS_FRAME_HEAD &&
                    sbus_rx_buffer[i + SBUS_FRAME_LEN - 1] == SBUS_FRAME_END)
                {
                    sbus_update_buffer(sbus_rx_buffer + i);
                    __HAL_UART_DISABLE_IT(&sbus_uart, UART_IT_RXNE);
                    break;
                }
            }
            sbus_rx_pos = 0; /* 防止越界 */
        }
    }

    rt_interrupt_leave();
}

// void SBUS_UART_DMA_RX_IRQHandler(void)
// {
//     rt_interrupt_enter();
//     HAL_DMA_IRQHandler(&sbus_rx_dma);
//     rt_interrupt_leave();
// }

/// @brief 电台串口字符接收中断处理函数
/// @param
void TELEGRAM_UART_HANDLER(void)
{
    rt_interrupt_enter();
    HAL_UART_IRQHandler(&telegram_uart);

    if (__HAL_UART_GET_FLAG(&telegram_uart, UART_FLAG_RXNE) == SET)
    {
        __HAL_UART_CLEAR_FLAG(&telegram_uart, UART_FLAG_RXNE);
        telegram_rx_buffer[telegram_rx_pos] = telegram_uart.Instance->DR & 0xff;
        rt_kprintf("%02x ", telegram_rx_buffer[telegram_rx_pos]);
        /* 解析出一包合法的mavlink帧 */
        if (telegram_resovle(telegram_rx_buffer[telegram_rx_pos]) == 0)
        {
            telegram_rx_pos = 0;
            __HAL_UART_DISABLE_IT(&telegram_uart, UART_IT_RXNE);
        }
        else
        {
            telegram_rx_pos++;
            /* 防止越界 */
            if (telegram_rx_pos == TELEGRAM_UART_RX_BUFFER_SIZE)
                telegram_rx_pos = 0;
        }
    }

    rt_interrupt_leave();
}

// void HAL_UART_TxCpltCallback(UART_HandleTypeDef *huart)
// {
//     if (huart == &telegram_uart)
//     {
//     }
// }

void TELEGRAM_UART_DMA_TX_IRQHandler(void)
{
    rt_interrupt_enter();
    HAL_DMA_IRQHandler(&telegram_tx_dma);
    rt_interrupt_leave();
}

// void TELEGRAM_UART_DMA_RX_IRQHandler(void)
// {
//     rt_interrupt_enter();
//     HAL_DMA_IRQHandler(&telegram_rx_dma);
//     rt_interrupt_leave();
// }

void GPS_UART_HANDLER(void)
{
    rt_interrupt_enter();
    HAL_UART_IRQHandler(&gps_uart);

    if (__HAL_UART_GET_FLAG(&gps_uart, UART_FLAG_RXNE) == SET)
    {
        __HAL_UART_CLEAR_FLAG(&gps_uart, UART_FLAG_RXNE);
        char ch = gps_uart.Instance->DR & 0xff;
        rt_kprintf("%c", ch);

        if (ch == '$')
        {
            gps_rx_pos = 0;
        }
        gps_rx_buffer[gps_rx_pos] = ch;
        gps_rx_pos++;

        if (ch == '\n')
        {
            /* 完成一帧NMEA0183获取 */
            update_nmea_data(gps_rx_buffer, gps_rx_pos);

            gps_rx_pos = 0;
            __HAL_UART_DISABLE_IT(&gps_uart, UART_IT_RXNE);
        }
    }

    rt_interrupt_leave();
}